// -*- coding: utf-8 -*-

#include "hybrid_ode_solver.h"
#include "hybrid_util.h"

using namespace std;
using namespace vnodelp;
using namespace v_bias;
using namespace v_blas;

USING_ELISA

const double	elisa::MinH(ODE_MIN_H);
const int		elisa::Ratio(ODE_RATIO);

bool HybridOdeSolver::dump(false);


void HybridOdeSolver::getBBox(bool left, const VNInterval& t, 
								  VNInterval& bT, VNIntVec& bAX) {
	// t下端 (上端) の状態を求める
	double t0 = left ? inf(t) + m_initTime.width() : sup(t);
	const VNIntVec x0(getEnclosure(t0));
	VNInterval	t1(t0);
	VNIVPtr		x(new VNIntVec(x0));

	// t上の区間包囲を計算する。
	VNIntVec	aX(x0);
	m_odeSolver.setFirstEntry();
	m_odeSolver.setHmax(/*width(m_initTime) +*/ MinH*Ratio);
	m_odeSolver.setOneStep(on);

	m_odeSolver.integrate(t1, *x, left ? sup(t) : inf(t));
	bT  = m_odeSolver.getT();
	bAX = m_odeSolver.getAprioriEncl();
}

void HybridOdeSolver::getMEncl(const VNInterval& t, double& midT, VNIntVec& midAX) {
	midT = t.midpoint();
	midAX = getEnclosure(midT);
}


/* HybridOdeSolverImpl */

const VNIntVec HybridOdeSolverImpl::getEnclosure(const double t) {
	// すでに計算しているかどうか
	VNIntPtr tp(new VNInterval(t));
	//PipeSet::iterator it(m_pipeSet.find(tp));
	PipeSet::iterator	it(m_pipeSet.lower_bound(tp));

	if (it != m_pipeSet.end()) {
		// TODO
		if (it != m_pipeSet.begin()) --it;
	}

	if (it != m_pipeSet.end() && sup(*it->first) == t) {

#ifdef _DEBUG
		cout << "cache hit!" << endl;
#endif

		VNEncl encl(it->second);
		return VNIntVec(*encl.second);

	} else {
		// tでの状態を計算する。

		// tの1つ前の状態を得る。
		bool b(false);
		PipeSet::iterator	it(m_pipeSet.lower_bound(tp));
/* TODO: これをやると解が正しくなくなる場合がある
*/
		while (it != m_pipeSet.begin()) {
			it--; 
			//if ((t - sup(*it->first)) >= MinH*Ratio) {
			if ((t - sup(*it->first)) >= MinH*Ratio) {
			// F if (t != sup(*it->first)) {
			//if (width(*it->first) <= MinH*Ratio) {
				b = true; 
				break;
			}
		}

		const VNInterval	t0(b ? sup(*it->first) : m_initTime);
		VNInterval			t1(t0);
		VNIVPtr				x( b ? VNIVPtr(new VNIntVec(*(it->second).second)) 
							     : VNIVPtr(new VNIntVec(m_initValue)));

		if (t - sup(t0) > 0) {
			m_odeSolver.setFirstEntry();
			m_odeSolver.setHmax(t - inf(t0));
			m_odeSolver.setOneStep(on);
#ifdef PIPE_CACHING
			VNIVPtr ax(new VNIntVec(*x));
#endif
			bool b(false);
			while (sup(t1) < t) {
				b = true;

				m_odeSolver.integrate(t1, *x, t);

#ifdef PIPE_CACHING
				// a priori包囲をためこむ。
				VNIVPtr		ax1(new VNIntVec(m_odeSolver.getAprioriEncl()));
				transform(ax->begin(), ax->end(), ax1->begin(), ax->begin(), vn::hull);

				// 計算結果を保存。
				VNIntPtr	at (new VNInterval(m_odeSolver.getT()));
				VNEncl		encl(ax1, VNIVPtr(new VNIntVec(*x)));
				VNPipe		p(at, encl);
				m_pipeSet.insert(p);
//cout << "saving t: " << *at << ", x: " << (*x)[0] << endl;

#ifdef HYBRID_DUMP
				if (dump) dumpPipe(odeOut, m_constr, *at, *ax1);
#endif
#endif
			}

#ifdef PIPE_CACHING
			if (b) {
				// 計算結果を保存。
				VNIntPtr at (new VNInterval(m_odeSolver.getT()));
				VNEncl encl(ax, x);
				//VNPipe p(tp, encl);
				VNPipe p(at, encl);
				m_pipeSet.insert(p);
			}

#ifdef HYBRID_DUMP
			if (dump) dumpPipe(odeOut, m_constr, *tp, *ax);
#endif
#endif
		}

		return VNIntVec(*x);
	}
}

const VNIntVec HybridOdeSolverImpl::getEnclosure(const VNInterval& t) {
	if (width(t) == 0)
		getEnclosure(inf(t));

	// すでに計算しているかどうか。
	VNIntPtr tp(new VNInterval(t));
	//PipeSet::iterator it(m_pipeSet.find(tp));
	PipeSet::iterator it(m_pipeSet.lower_bound(tp));
	for (; it != m_pipeSet.end(); it--) {
		if (sup(*it->first) != sup(t))
			break;
		else {
			/*VNInterval t1(*it->first);
			if (width(t1) > 0 && (inf(t1)-inf(t)) > MinH*Ratio) {
				// 短いpipeがあった。
				VNIntVec ax(*it->second.first);
				VNIntVec ax1(getEnclosure(VNInterval(inf(t), inf(t1))));
				transform(ax.begin(), ax.end(), ax1.begin(), ax.begin(), vn::hull);
				return ax;

			} else */
			if (inf(*it->first) == inf(t)) {			// ピッタリなのがあった。
#ifdef _DEBUG
			cout << "cache hit!!" << endl;
#endif 
				return VNIntVec(*it->second.first);
			}
		}
	}

	// t上の区間包囲を計算する。
	VNInterval	t1(inf(t));
	const VNIntVec x0(getEnclosure(inf(t)));
	VNIVPtr		x(new VNIntVec(x0));
	VNIVPtr		ax(new VNIntVec(x0));
	m_odeSolver.setHmax(width(t));
	m_odeSolver.setOneStep(on);
	m_odeSolver.setFirstEntry();
	while (sup(t1) < sup(t)) {
		m_odeSolver.integrate(t1, *x, sup(t));

		// a priori包囲をためこむ。
		VNIVPtr		ax1(new VNIntVec(m_odeSolver.getAprioriEncl()));
		transform(ax->begin(), ax->end(), ax1->begin(), ax->begin(), vn::hull);

/* TODO
#ifdef PIPE_CACHING
		// 計算結果を保存。
		VNIntPtr	at (new VNInterval(m_odeSolver.getT()));
		VNEncl		encl(ax1, VNIVPtr(new VNIntVec(*x)));
		VNPipe		p(at, encl);
		m_pipeSet.insert(p);

#ifdef HYBRID_DUMP
		if (dump) dumpPipe(odeOut, m_constr, *at, *ax1);
#endif
#endif
*/
	}

#ifdef PIPE_CACHING
	// 計算結果を保存。
	VNEncl	encl(ax, x);
	VNPipe	p(tp, encl);
	m_pipeSet.insert(p);

#ifdef HYBRID_DUMP
	if (dump) dumpPipe(odeOut, m_constr, *tp, *ax);
#endif
#endif

	return VNIntVec(*ax);
}


#ifdef USE_PIPE_HASHMAP
/* HybridOdeSolverImpl1 */

const VNIntVec HybridOdeSolverImpl1::getEnclosure(const double t) {
	// すでに計算しているかどうか
	VNIntPtr tp(new VNInterval(t));
	PipeSet::iterator it(m_pipeSet.find(t));

	if (it != m_pipeSet.end()) {
/*#ifdef _DEBUG
		cout << "cache hit!" << endl;
#endif*/
		VNEncl encl(it->second.second);
		return (*encl.second);

	} else {
		// tでの状態を計算する。

		// tの1つ前の状態を得る。
		bool b(false);
		const VNInterval	t0(m_initTime);
		VNInterval			t1(t0);
		VNIVPtr				x(VNIVPtr(new VNIntVec(m_initValue)));

		if (t - sup(t0) > 0) {
			m_odeSolver.setFirstEntry();
			m_odeSolver.setHmax(t - inf(t0));
			m_odeSolver.setOneStep(on);
			VNIVPtr ax(new VNIntVec(*x));
			while (sup(t1) < t) {
				m_odeSolver.integrate(t1, *x, t);

				// a priori包囲をためこむ。
				VNIVPtr		ax1(new VNIntVec(m_odeSolver.getAprioriEncl()));
				transform(ax->begin(), ax->end(), ax1->begin(), ax->begin(), vn::hull);

#ifdef PIPE_CACHING
				// 計算結果を保存。
				VNIntPtr	at (new VNInterval(m_odeSolver.getT()));
				VNEncl		encl(ax1, VNIVPtr(new VNIntVec(*x)));
				VNPipe		p(at, encl);
				PipeExt		pe(sup(*at), p);
				m_pipeSet.insert(pe);
#endif
			}

#ifdef PIPE_CACHING
			// 計算結果を保存。
			VNIntPtr	at (new VNInterval(m_odeSolver.getT()));
			VNEncl		encl(ax, x);
			VNPipe		p(at, encl);
			PipeExt		pe(sup(*tp), p);
			m_pipeSet.insert(pe);
#endif
		}

		return *x;
	}
}

const VNIntVec HybridOdeSolverImpl1::getEnclosure(const VNInterval& t) {
	if (width(t) == 0)
		getEnclosure(inf(t));

	// すでに計算しているかどうか。
	PipeSet::iterator it(m_pipeSet.find(sup(t)));
	for (; it != m_pipeSet.end(); it++) {
		if (it->first != sup(t))
			break;
		
		else {
/*#ifdef _DEBUG
			cout << "cache hit!!" << endl;
#endif*/
			VNInterval t1(*it->second.first);
			if (width(t1) > 0 && (inf(t1)-inf(t)) > MinH*Ratio) {
				// 短いpipeがあった
				VNIntVec ax(*it->second.second.first);
				VNIntVec ax1(getEnclosure(VNInterval(inf(t), inf(t1))));
				transform(ax.begin(), ax.end(), ax1.begin(), ax.begin(), vn::hull);
				return ax;

			} else if (it->first == inf(t)) { // ピッタリなのがあった。
				return *it->second.second.first;
			}
		}
	}

	// t上の区間包囲を計算する。
	VNInterval	t1(inf(t));
	const VNIntVec x0(getEnclosure(inf(t)));
	VNIVPtr		x(new VNIntVec(x0));
	VNIVPtr		ax(new VNIntVec(x0));
	m_odeSolver.setHmax(width(t));
	m_odeSolver.setOneStep(on);
	m_odeSolver.setFirstEntry();
	while (sup(t1) < sup(t)) {
		m_odeSolver.integrate(t1, *x, sup(t));

		// a priori包囲をためこむ。
		VNIVPtr		ax1(new VNIntVec(m_odeSolver.getAprioriEncl()));
		transform(ax->begin(), ax->end(), ax1->begin(), ax->begin(), vn::hull);

#ifdef PIPE_CACHING
		// 計算結果を保存。
		VNIntPtr	tp (new VNInterval(m_odeSolver.getT()));
		VNEncl		encl(ax1, VNIVPtr(new VNIntVec(*x)));
		VNPipe		p(tp, encl);
		PipeExt		pe(sup(*tp), p);
		m_pipeSet.insert(pe);
#endif
	}

#ifdef PIPE_CACHING
	// 計算結果を保存。
	VNIntPtr	tp(new VNInterval(t));
	VNEncl		encl(ax, x);
	VNPipe		p(tp, encl);
	PipeExt		pe(sup(*tp), p);
	m_pipeSet.insert(pe);
#endif

	return *ax;
}
#endif // USE_PIPE_HASHMAP
